/****************************************************************************
 * Copyright (C) 2025 Chenyz. All rights reserved.
 *
 * @file    TinyOS_Switch.c
 *
 * @par     dependencies
 *
 * @author  Chenyz
 *
 * @brief   
 *
 * @version V1.0
 * @date    2025-09-4
 *
 * @note    
 ****************************************************************************/
#include "TinyOS_Switch.h"
#include "stm32f10x.h"
/******************************** Defines **********************************/
#define     ICSR_ADDR           (0xE000ED04)
#define     PENFSV_INT_SET_BIT  (1<<28)
#define     SHPR_ADDR           (0xE000ED22)
#define     PENFSV_PRIORITY_SET (0x000000FF)

#define     MEN32(ADDR)     *(uint32_t *)(ADDR)
#define     MEN8(ADDR)      *(uint8_t *)(ADDR)

uint32_t TinyOS_TaskEnterCritical(void)
{
	uint32_t Primask = __get_PRIMASK();
	__disable_irq();
	return Primask;
}

void TinyOS_TaskExitCritical(uint32_t Status)
{
	__set_PRIMASK(Status);
}


__asm void PendSV_Handler(void)
{
	IMPORT CurrentTask
	IMPORT NextTask
	
	MRS R0, PSP
	CBZ R0, PendSV_Handler_NoSave
	
	STMDB R0!, {R4-R11}
	LDR R1, = CurrentTask
	LDR R1, [R1]
	STR R0, [R1]
	
PendSV_Handler_NoSave
	LDR R0, = CurrentTask
	LDR R1, = NextTask
	LDR R2, [R1]
	STR R2, [R0]
	
	LDR R0, [R2]
	
	LDMIA R0!, {R4-R11}
	MSR PSP ,R0
	
	ORR LR, LR, #0x04
	BX LR

}

void TinyOS_TaskSwitch(void)
{
	MEN32(ICSR_ADDR) = PENFSV_INT_SET_BIT;
}


void TinyOS_TaskRunFisrt(void)
{
	__set_PSP(0);
	MEN8(SHPR_ADDR) = PENFSV_PRIORITY_SET;
    MEN32(ICSR_ADDR) = PENFSV_INT_SET_BIT;	
}



